/************************************************************************************
 * WrightEagle (Soccer Simulation League 2D)                                        *
 * BASE SOURCE CODE RELEASE 2010                                                    *
 * Copyright (c) 1998-2010 WrightEagle 2D Soccer Simulation Team,                   *
 *                         Multi-Agent Systems Lab.,                                *
 *                         School of Computer Science and Technology,               *
 *                         University of Science and Technology of China            *
 * All rights reserved.                                                             *
 *                                                                                  *
 * Redistribution and use in source and binary forms, with or without               *
 * modification, are permitted provided that the following conditions are met:      *
 *     * Redistributions of source code must retain the above copyright             *
 *       notice, this list of conditions and the following disclaimer.              *
 *     * Redistributions in binary form must reproduce the above copyright          *
 *       notice, this list of conditions and the following disclaimer in the        *
 *       documentation and/or other materials provided with the distribution.       *
 *     * Neither the name of the WrightEagle 2D Soccer Simulation Team nor the      *
 *       names of its contributors may be used to endorse or promote products       *
 *       derived from this software without specific prior written permission.      *
 *                                                                                  *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND  *
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED    *
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE           *
 * DISCLAIMED. IN NO EVENT SHALL WrightEagle 2D Soccer Simulation Team BE LIABLE    *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL       *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR       *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER       *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,    *
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF *
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                *
 ************************************************************************************/

#include "BehaviorFormation.h"
#include "VisualSystem.h"
#include "Formation.h"
#include "Dasher.h"
#include "BasicCommand.h"
#include "BehaviorPosition.h"
#include "Agent.h"
#include "PositionInfo.h"
#include "Logger.h"
#include <cstdlib>
#include "Evaluation.h"

const BehaviorType BehaviorFormationExecuter::BEHAVIOR_TYPE = BT_Formation;

namespace
{
bool ret = BehaviorExecutable::AutoRegister<BehaviorFormationExecuter>();
}

BehaviorFormationExecuter::BehaviorFormationExecuter(Agent & agent) :
	BehaviorExecuterBase<BehaviorDefenseData>(agent)
{
	Assert(ret);
}

BehaviorFormationExecuter::~BehaviorFormationExecuter(void)
{
}

bool BehaviorFormationExecuter::Execute(const ActiveBehavior & beh)
{
	Logger::instance().LogGoToPoint(mSelfState.GetPos(), beh.mTarget, "@Formation");
  std::cout<<"dgk execute_formation"<<std::endl;
	return Dasher::instance().GoToPoint(mAgent, beh.mTarget, beh.mBuffer, beh.mPower, false, true);
}

BehaviorFormationPlanner::BehaviorFormationPlanner(Agent & agent):
	BehaviorPlannerBase<BehaviorDefenseData>( agent)
{
}

BehaviorFormationPlanner::~BehaviorFormationPlanner()
{
}

void BehaviorFormationPlanner::Plan(std::list<ActiveBehavior> & behavior_list)
{
  Vector initialPosition=mSelfState.GetPos();
  std::cout<<"dgk initial_formation_position "<<initialPosition.X()<<" "<<initialPosition.Y()<<std::endl;
	ActiveBehavior formation(mAgent, BT_Formation);

	formation.mBuffer = 1.0;
	formation.mPower = mSelfState.CorrectDashPowerForStamina(ServerParam::instance().maxDashPower());
	formation.mTarget = mAnalyser.mHome[mSelfState.GetUnum()];
	if(formation.mTarget.X() < mSelfState.GetPos().X() && mAgent.GetFormation().GetMyRole().mLineType == LT_Forward){
		formation.mPower = mSelfState.CorrectDashPowerForStamina(ServerParam::instance().maxDashPower())/2;
	}


	if(mAgent.GetFormation().GetMyRole().mLineType == LT_Defender){
		formation.mTarget.SetX(Min(0.0,formation.mTarget.X()));
		if(mBallState.GetPos().X() < -25){
			Unum goalie = mWorldState.GetTeammateGoalieUnum();
			Vector gpos = mWorldState.GetTeammate(goalie).GetPos();
			Vector left = (ServerParam::instance().ourLeftGoalPost() + gpos)/2;
			Vector right = (ServerParam::instance().ourRightGoalPost() + gpos)/2;
			Line l(gpos,mBallState.GetPos());
			if(l.IsPointInSameSide(left,ServerParam::instance().ourGoal())){
			const std::vector<Unum> & t2t = mPositionInfo.GetClosePlayerToPoint(left,goalie);
			if(t2t[0] == mSelfState.GetUnum()){
				formation.mTarget = left;
			}
			else if(mPositionInfo.GetClosePlayerToPoint(right,t2t[0])[0] == mSelfState.GetUnum()){
				formation.mTarget = right;
			} else if(mPositionInfo.GetClosePlayerToPoint(right,t2t[0])[0] == goalie && mPositionInfo.GetClosePlayerToPoint(right,t2t[0])[1] == mSelfState.GetUnum()){
				formation.mTarget = right;
			}
			}
			else {
				const std::vector<Unum> & t2t = mPositionInfo.GetClosePlayerToPoint(right,goalie);
				if(t2t[0] == mSelfState.GetUnum()){
					formation.mTarget = right;
				}
				else if(mPositionInfo.GetClosePlayerToPoint(left,t2t[0])[0] == mSelfState.GetUnum()){
					formation.mTarget = left;
				} else if(mPositionInfo.GetClosePlayerToPoint(left,t2t[0])[0] == goalie && mPositionInfo.GetClosePlayerToPoint(right,t2t[0])[1] == mSelfState.GetUnum()){
					formation.mTarget = left;
				}
			}

		}
		formation.mEvaluation = Evaluation::instance().EvaluatePosition(formation.mTarget, true);
	}

	else formation.mEvaluation = Evaluation::instance().EvaluatePosition(formation.mTarget, false);

  std::cout<<"dgk planned_formation "<<formation.mTarget.X()<<" "<<formation.mTarget.Y()<<std::endl;
  std::cout<<"dgk planned_formation_evaluation "<<formation.mEvaluation<<std::endl;
  //force fields
  double di_ball=dist(mSelfState.GetPos().X(), mSelfState.GetPos().Y(), mBallState.GetPos().X(), mBallState.GetPos().Y());
  double B[2], strengthB=1.0;
  forceFieldB(B, &initialPosition);
  double O[2], strengthO=1.0;
  forceFieldO(O, &initialPosition);
  if(softIf(di_ball, 20, 10)) {
    //close to ball - tactical
    double T[2], strengthT=1.0;
    forceFieldT(T, &initialPosition);

    double C[2], strengthC=1.0;
    forceFieldC(C);

    std::cout<<"dgk strength T "<<strengthT<<" C "<<strengthC<<std::endl;
    formation.mTarget.SetX(formation.mTarget.X()+strengthB*B[0]+strengthO*O[0]+strengthT*T[0]+strengthC*C[0]);
    formation.mTarget.SetY(formation.mTarget.Y()+strengthB*B[1]+strengthO*O[1]+strengthT*T[1]+strengthC*C[1]);
  } else {
    //far from ball - strategic
    double S[2], strengthS=1.0;
    forceFieldS(S, &initialPosition);

    std::cout<<"dgk strength B "<<strengthB<<" O "<<strengthO<<" S "<<strengthS<<std::endl;
    formation.mTarget.SetX(formation.mTarget.X()+strengthB*B[0]+strengthO*O[0]+strengthS*S[0]);
    formation.mTarget.SetY(formation.mTarget.Y()+strengthB*B[1]+strengthO*O[1]+strengthS*S[1]);
  }
  formation.mEvaluation = Evaluation::instance().EvaluatePosition(formation.mTarget, false);
  std::cout<<"dgk forcefield_formation "<<formation.mTarget.X()<<" "<<formation.mTarget.Y()<<std::endl;
  std::cout<<"dgk forcefield_formation_evaluation "<<formation.mEvaluation<<std::endl;
  // -- force fields
	behavior_list.push_back(formation);
}

bool BehaviorFormationPlanner::softIf(double left, double right, double delta) {
  int val=(int)floor(0.5+1.0/(1+exp(-(right-left)/delta)));
  return val==0;
}

bool BehaviorFormationPlanner::softEq(double left, double right, double delta) {
  int val=(int)floor(0.5+exp(-pow(left-right, 2.0)/pow(delta, 2.0)));
  return val==0;
}

double BehaviorFormationPlanner::dist(double x1, double y1, double x2, double y2) {
  return sqrt(pow(x1-x2, 2)+pow(y1-y2, 2));
}

void BehaviorFormationPlanner::forceFieldB(double *B, Vector *initialPosition) {
  //B: bounds-repellent force field
  //from WorldState.LimitToField
  //X = from -ServerParam::instance().PITCH_LENGTH/2 to ServerParam::instance().PITCH_LENGTH/2 = -52.5 52.5
  //Y = from -ServerParam::instance().PITCH_WIDTH/2 to ServerParam::instance().PITCH_WIDTH/2 = -34 34
  B[0]=initialPosition->X();
  double pitch_length=ServerParam::instance().PITCH_LENGTH/2;
  //avoid /0
  if(B[0]<=-pitch_length) {
    B[0]=-(pitch_length-0.1);
  }
  else if(B[0]>=pitch_length) {
    B[0]=pitch_length-0.1;
  }
  B[0]=5.0/(B[0]+pitch_length)-5.0/(pitch_length-B[0]);
  B[1]=initialPosition->Y();
  double pitch_width=ServerParam::instance().PITCH_WIDTH/2;
  std::cout<<"dgk debug B length "<<pitch_length<<" width "<<pitch_width<<std::endl;
  //avoid /0
  if(B[1]<=-pitch_width) {
    B[1]=-(pitch_width-0.1);
  }
  else if(B[1]>=pitch_width) {
    B[1]=pitch_width-0.1;
  }
  B[1]=5.0/(B[1]+pitch_width)-5.0/(pitch_width-B[1]);
  //capping
  B[0]=B[0]>5.0 ? 5.0 : B[0];
  B[0]=B[0]<-5.0 ? -5.0 : B[0];
  B[1]=B[1]>5.0 ? 5.0 : B[1];
  B[1]=B[1]<-5.0 ? -5.0 : B[1];
  std::cout<<"dgk formation_B "<<B[0]<<" "<<B[1]<<std::endl;
  // -- B: bounds-repellent force field
}

void BehaviorFormationPlanner::forceFieldO(double *O, Vector *initialPosition) {
  //O: offsides-repellent force field
  //if(mPositionInfo.GetOpponentOffsideLine() > mBallState.GetPos().X()){
  //    position.mTarget.SetX(Min(target.X(),(mPositionInfo.GetOpponentOffsideLine())));
  //  }
  O[0]=0.0;
  O[1]=0.0;
  //less than 5m from offside line
  std::cout<<"dgk debug O offsideline "<<mPositionInfo.GetOpponentOffsideLine()<<std::endl;
  if(softIf(abs(mPositionInfo.GetOpponentOffsideLine()-initialPosition->X()), 5.0, 1.0)) {
    //which way shoul I go?
    double goalieX=mWorldState.GetPlayer(mWorldState.GetTeammateGoalieUnum()).GetPos().X();
    std::cout<<"dgk debug O goalieX "<<goalieX<<std::endl;
    if(goalieX>0) {
      if(initialPosition->X()<mPositionInfo.GetOpponentOffsideLine()) {
        O[0]=5.0;
      }
    } else {
      if(initialPosition->X()>mPositionInfo.GetOpponentOffsideLine()) {
        O[0]=-5.0;
      }
    }
  }
  std::cout<<"dgk formation_O "<<O[0]<<" "<<O[1]<<std::endl;
  // -- O: offsides-repellent force field
}

void BehaviorFormationPlanner::forceFieldS(double *S, Vector *initialPosition) {
  //S: inter-player force field
  //mPositionInfo.GetCloseTeammateToPlayer(Unum i);
  //replace closest by GetCloseTeammateToPlayer
  Vector posTeammate=mWorldState.GetPlayer(mPositionInfo.GetClosestTeammateToPlayer(mSelfState.GetUnum())).GetPos();
  double di=dist(initialPosition->X(), initialPosition->Y(), posTeammate.X(), posTeammate.Y());
  std::cout<<"dgk debug S teammate "<<posTeammate.X()<<" "<<posTeammate.Y()<<" di "<<di<<std::endl;
  S[0]=0.0;
  S[1]=0.0;
  if(softEq(di, 20, 10)) {
    S[0]=posTeammate.X()-initialPosition->X();
    S[1]=posTeammate.Y()-initialPosition->Y();
    double norm=sqrt(pow(S[0], 2.0)+pow(S[1], 2.0));
    //2 is the strenght of the field
    S[0]=2*S[0]/norm;
    S[1]=2*S[1]/norm;
    if(softIf(di, 20, 10)) {
      //repel
      S[0]=-S[0];
      S[1]=-S[1];
    }
  }
  std::cout<<"dgk formation_S "<<S[0]<<" "<<S[1]<<std::endl;
  // -- S: inter-player force field
}

void BehaviorFormationPlanner::forceFieldT(double *T, Vector *initialPosition) {
  //T: tactical inter-player force field
  T[0]=0.0;
  T[1]=0.0;
  Vector posTeammate=mWorldState.GetPlayer(mPositionInfo.GetClosestTeammateToPlayer(mSelfState.GetUnum())).GetPos();
  double di=dist(initialPosition->X(), initialPosition->Y(), posTeammate.X(), posTeammate.Y());
  if(softIf(di, 8, 3)) {
    T[0]=posTeammate.X()-initialPosition->X();
    T[1]=posTeammate.Y()-initialPosition->Y();
    double norm=sqrt(pow(T[0], 2.0)+pow(T[1], 2.0));
    T[0]=-5.0*T[0]/norm;
    T[1]=-5.0*T[1]/norm;
  }
  std::cout<<"dgk formation_T "<<T[0]<<" "<<T[1]<<std::endl;
  // -- T: tactical inter-player force field
}

void BehaviorFormationPlanner::forceFieldC(double *C) {
  //C: get-clear force field
  //closest opponent and ball
  Vector posOpponent=mWorldState.GetPlayer(mPositionInfo.GetClosestOpponentToPlayer(mSelfState.GetUnum())).GetPos();
  Vector posBall=mBallState.GetPos();
  double obx=posOpponent.X()-posBall.X();
  double oby=posOpponent.Y()-posBall.Y();
  //C is orthogonal to the vector defined by Opponent-Ball; dot product = 0
  C[0]=obx;
  C[1]=-obx*C[0]/oby;
  double norm=sqrt(pow(C[0], 2.0)+pow(C[1], 2.0));
  C[0]=C[0]/norm;
  C[1]=C[1]/norm;
  std::cout<<"dgk formation_C "<<C[0]<<" "<<C[1]<<std::endl;
  // -- C: get-clear force field
}
